#include "UniverseModel.h"

#include "BodyFactory.h"
#include "pw_exception.h"
#include "math/aux_funcs.h"
#include <cassert>
#include <cstring>
#include <boost/scoped_array.hpp>

namespace physicsworks {

using exception::IBadArg;
using exception::IWorldAlreadyAdded;
using exception::IWorldBodyAlreadyAdded;
using exception::IUnkownWorld;

static Universe* universe = 0;

Universe* instanceUniverse()
{
	if (universe == 0) universe = new UniverseModel;
	assert(universe != 0);
	return universe;
}

void destroyUniverse()
{
	if (universe == 0) return;
	delete universe;
	universe = 0;
	WorldFactory::destroy();
	BodyFactory::destroy();
}

bool universeAlive()
{
	return universe;
}

void UniverseModel::add(World* w)
{
	typedef WorldModel::RigidBodyHSet RigidBodyHSet;
	
	WorldModel* wm = dynamic_cast<WorldModel*>(w);
	Assert<IBadArg>(wm, "Invalid world argument");
	
	Assert<IWorldAlreadyAdded>(worldModels.find(wm) == worldModels.end(), std::string("The world ") + w->getName() + " was already added");
	
	const RigidBodyHSet rb = wm->getBodiesListHSet();
	
	WorldModelHSet::const_iterator wmiter;
	for (wmiter = worldModels.begin(); wmiter != worldModels.end(); ++wmiter) {
		
		const RigidBodyHSet arb = (*wmiter)->getBodiesListHSet();
		
		RigidBodyHSet::const_iterator rbiter;
		for (rbiter = rb.begin(); rbiter != rb.end(); ++rbiter) {
			
			Assert<IWorldBodyAlreadyAdded>(arb.find(*rbiter) == arb.end(), std::string("The world ") + (*wmiter)->getName()
			                               + " has at least one body" + " in common with this world");
		}
		
	}
	
	worldModels.insert(wm);
}

void UniverseModel::remove(World* w)
{
	WorldModel* wm = dynamic_cast<WorldModel*>(w);
	Assert<IBadArg>(wm, "Invalid world argument");
	
	size_t s = worldModels.erase(wm);
	Assert<IUnkownWorld>(s, std::string("The world ") + w->getName() + " was not added");
}

World** UniverseModel::getWorldsList(int* size)
{
	*size = worldModels.size();
	if (*size == 0)
		return 0;
	
	World** w = new World*[*size];
	
	WorldModelHSet::const_iterator iter;
	int i = 0;
	for (iter = worldModels.begin(); iter != worldModels.end(); ++iter) {
		w[i++] = *iter;
	}
	return w;
}

World* UniverseModel::instanceWorld(real aX, real aY, real aZ, real bX, real bY, real bZ,
                                    SpatialPartitioningType s, const SpatialPartitioningConfig& sc) const
{
	using math::equal;
	Assert<IBadArg>(!equal<real>(aX - bX, 0) && !equal<real>(aY - bY, 0) &&
	                !equal<real>(aZ - bZ, 0), "The a and b vectors must represent a box");
	
	vec3 a(aX, aY, aZ);
	vec3 b(bX, bY, bZ);
	
	return WorldFactory::instance()->instance(a, b, s, sc);
}

Body* UniverseModel::instanceBody(const real* vertX, const real* vertY, const real* vertZ, int numVertices,
	                              const int* verticesIndices, int numFaces) const
{
	vec3* vertices = new vec3[numVertices];
	boost::scoped_array<int> vertIndices(new int[numFaces * 3]);
	
	for (int i = 0; i < numVertices; ++i) {
		vertices[i].x = vertX[i];
		vertices[i].y = vertY[i];
		vertices[i].z = vertZ[i];
	}
	memcpy(vertIndices.get(), verticesIndices, numFaces * 3 * sizeof(int));
	
	Body* b;
	try {
		b = BodyFactory::instance()->instanceRigidBody(vertices, numVertices, vertIndices.get(), numFaces);
	} catch (...) {
		delete [] vertices;
		throw;
	}
	
	return b;
}

Body* UniverseModel::instanceSphereBody(real radius) const
{
	return BodyFactory::instance()->instanceSphere(radius);
}

Body* UniverseModel::instanceCubeBody(real radius) const
{
	return BodyFactory::instance()->instanceCube(radius);
}

Body* UniverseModel::instanceBoxBody(real radiusX, real radiusY, real radiusZ) const
{
	return BodyFactory::instance()->instanceBox(vec3(radiusX, radiusY, radiusZ));
}

}
